# -*-coding:utf-8-*-
# Copyright (c) 2020 DJI.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License in the file LICENSE.txt or at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.


import time
from robomaster import robot
from robomaster import camera
import rospy
import cv2
from sensor_msgs.msg import Image
import numpy as np

def publish_im0():
    ep_robot = robot.Robot()
    ep_robot.initialize(conn_type="sta")
    ep_camera = ep_robot.camera
    rospy.init_node("zt_camera")

    # 设置视频流参数
    ep_camera.start_video_stream(display=False, resolution=camera.STREAM_720P)

    rate = rospy.Rate(5)  # 设置发布频率，例如每30帧发布一次
    for i in range(10):
        img = ep_camera.read_cv2_image(timeout=3, strategy="newest")
        if img is not None:
            # 发布图像消息
            rospy.loginfo("Publishing image")
            pub = rospy.Publisher('im0', Image, queue_size=10)
            cv_image = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
            msg = Image()
            msg.data = np.array(cv_image).tobytes()
            msg.height = img.shape[0]
            msg.width = img.shape[1]
            msg.step = img.shape[2]
            pub.publish(msg)
        rate.sleep()

    # 停止视频流
    ep_camera.stop_video_stream()
    ep_robot.close()

if __name__ == '__main__':
    try:
        publish_im0()
    except rospy.ROSInterruptException:
        pass